"""
--------------------------------------------------------------------------------
功能:机械臂使用直线插补向前向后运动
日期:2025-09-07
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
--------------------------------------------------------------------------------
"""

import time
import queue
import threading
from src.arm_control import manager
from src.arm_control import custom_font

#=======================================加载自定义字体
time.sleep(1)
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
print(font.color_text(font.font_load_success, color="green", bold=True))

#=======================================创建机械臂实例,以及机械臂相关参数
arm = manager.RobotManager()
arm.arm_init_action()#机械臂执行初始化动作
print(font.color_text(font.arm_load_success, color="green", bold=True))


detection_red_count = 0
detection_green_count = 0
while True:
    try:
        arm.arm_move(50.0, 0.0, 80.0, 20)
        time.sleep(1)
        arm.arm_move(120.0, 0.0, 80.0, 20)
        time.sleep(1)
    except KeyboardInterrupt:
        print("\n程序退出")
        break
    except Exception as e:
        time.sleep(1)  # 出错时等待1秒再继续
        arm.arm_init_action()

